/* Project: Gipsa-lab plugins for OpenVibe
 * AUTHORS AND CONTRIBUTORS: Andreev A., Barachant A., Congedo M., Ionescu,Gelu,

 * This file is part of "Gipsa-lab plugins for OpenVibe".
 * You can redistribute it and/or modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
 * This file is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License along with Brain Invaders. If not, see http://www.gnu.org/licenses/.*/
 
#include "ovpCBoxAlgorithmProcessMDM.h"

#include <iostream>
#include <sstream>
#include <system/Memory.h>

#include <boost/algorithm/string.hpp>

#include "ovpRiemannHelper.h"

using namespace OpenViBE;
using namespace OpenViBE::Kernel;
using namespace OpenViBE::Plugins;

using namespace OpenViBEPlugins;
using namespace OpenViBEPlugins::SignalProcessing;

boolean CBoxAlgorithmProcessMDM::initialize(void)
{
	// init stimulation OUTPUT 1
	m_pStimulationEncoder1=&this->getAlgorithmManager().getAlgorithm(this->getAlgorithmManager().createAlgorithm(OVP_GD_ClassId_Algorithm_StimulationStreamEncoder));
	m_pStimulationEncoder1->initialize();
	ip_pStimulationsToEncode1.initialize(m_pStimulationEncoder1->getInputParameter(OVP_GD_Algorithm_StimulationStreamEncoder_InputParameterId_StimulationSet));
	op_pEncodedMemoryBuffer1.initialize(m_pStimulationEncoder1->getOutputParameter(OVP_GD_Algorithm_StimulationStreamEncoder_OutputParameterId_EncodedMemoryBuffer));

	// init INPUT signal decoder
	m_pSignalDecoder=&this->getAlgorithmManager().getAlgorithm(this->getAlgorithmManager().createAlgorithm(OVP_GD_ClassId_Algorithm_StreamedMatrixStreamDecoder));
	m_pSignalDecoder->initialize();
	ip_pMemoryBufferToDecode1.initialize(m_pSignalDecoder->getInputParameter(OVP_GD_Algorithm_StreamedMatrixStreamDecoder_InputParameterId_MemoryBufferToDecode));
	op_pDecodedMatrix1.initialize(m_pSignalDecoder->getOutputParameter(OVP_GD_Algorithm_StreamedMatrixStreamDecoder_OutputParameterId_Matrix));

	// init output signal encoder 1 - score
	m_pSignalEncoder=&this->getAlgorithmManager().getAlgorithm(this->getAlgorithmManager().createAlgorithm(OVP_GD_ClassId_Algorithm_StreamedMatrixStreamEncoder));
	m_pSignalEncoder->initialize();
	ip_pMatrixToEncode.initialize(m_pSignalEncoder->getInputParameter(OVP_GD_Algorithm_StreamedMatrixStreamEncoder_InputParameterId_Matrix));
	op_pEncodedMemoryBuffer.initialize(m_pSignalEncoder->getOutputParameter(OVP_GD_Algorithm_StreamedMatrixStreamEncoder_OutputParameterId_EncodedMemoryBuffer));

	//> init stimulation output 2 - filtered stimulations
	m_pStimulationEncoder2=&this->getAlgorithmManager().getAlgorithm(this->getAlgorithmManager().createAlgorithm(OVP_GD_ClassId_Algorithm_StimulationStreamEncoder));
	m_pStimulationEncoder2->initialize();
	ip_pStimulationsToEncode2.initialize(m_pStimulationEncoder2->getInputParameter(OVP_GD_Algorithm_StimulationStreamEncoder_InputParameterId_StimulationSet));
	op_pEncodedMemoryBuffer2.initialize(m_pStimulationEncoder2->getOutputParameter(OVP_GD_Algorithm_StimulationStreamEncoder_OutputParameterId_EncodedMemoryBuffer));

	m_vClassStimulations.clear();
	m_vOnlineClasses.clear();

	//UI parameters:
	IBox& l_rStaticBoxContext=this->getStaticBoxContext();
	CString l_sSettingValue;

	//Parameters file path
	l_rStaticBoxContext.getSettingValue(0, l_sSettingValue);
	std::string l_sFileNamePath=(std::string)this->getConfigurationManager().expand(l_sSettingValue);

	//IsP300
	l_rStaticBoxContext.getSettingValue(1, l_sSettingValue);
	m_IsP300=(OpenViBE::boolean)this->getConfigurationManager().expandAsBoolean(l_sSettingValue);

	OpenViBE::boolean fileLoadedSuccessfully = loadFile(l_sFileNamePath);
	if (!fileLoadedSuccessfully) return false;

	m_bHasSentHeader = false;

	//read the stimulations for each class
	for (int i=2;i<l_rStaticBoxContext.getSettingCount();i++)
	{
		m_vClassStimulations.push_back(FSettingValueAutoCast(*this->getBoxAlgorithmContext(), i));
	}

	//this->getLogManager() << LogLevel_Info << "Process MDM init finished.\n";

	this->getLogManager() << LogLevel_Info << "Total number of classes: " << (OpenViBE::uint32)m_vOnlineClasses.size() << "\n";
	
	return true;
}

boolean CBoxAlgorithmProcessMDM::uninitialize(void)
{
	//uninit INPUT signal decoder
	op_pDecodedMatrix1.uninitialize();
	ip_pMemoryBufferToDecode1.uninitialize();
	m_pSignalDecoder->uninitialize();
	this->getAlgorithmManager().releaseAlgorithm(*m_pSignalDecoder);

	//uninit stimulation output 1 
	m_pStimulationEncoder1->uninitialize();
	ip_pStimulationsToEncode1.uninitialize();
	op_pEncodedMemoryBuffer1.uninitialize();
	this->getAlgorithmManager().releaseAlgorithm(*m_pStimulationEncoder1);

	// uninit stimulation output 2 
	m_pStimulationEncoder2->uninitialize();
	ip_pStimulationsToEncode2.uninitialize();
	op_pEncodedMemoryBuffer2.uninitialize();
	this->getAlgorithmManager().releaseAlgorithm(*m_pStimulationEncoder2);

	//uninit output signal encoder - score
	ip_pMatrixToEncode.uninitialize();
	op_pEncodedMemoryBuffer.uninitialize();
	m_pSignalEncoder->uninitialize();
	this->getAlgorithmManager().releaseAlgorithm(*m_pSignalEncoder);

    m_vClassStimulations.clear();
	for (int i=0;i<m_vOnlineClasses.size();i++)
	{
	   delete m_vOnlineClasses[i];
	}
	m_vOnlineClasses.clear();
	
	return true;
}

boolean CBoxAlgorithmProcessMDM::processInput(uint32 ui32InputIndex)
{
	getBoxAlgorithmContext()->markAlgorithmAsReadyToProcess();
	return true;
}

boolean CBoxAlgorithmProcessMDM::process(void)
{
	IBox& l_rStaticBoxContext=this->getStaticBoxContext();
	IBoxIO* l_rDynamicBoxContext=getBoxAlgorithmContext()->getDynamicBoxContext();

	for(int i=0; i<l_rDynamicBoxContext->getInputChunkCount(0); i++)
	{
		TParameterHandler<const IMemoryBuffer*> ip_pMemoryBuffer(m_pSignalDecoder->getInputParameter(OVP_GD_Algorithm_SignalStreamDecoder_InputParameterId_MemoryBufferToDecode));
		ip_pMemoryBuffer=l_rDynamicBoxContext->getInputChunk(0, i);
		m_pSignalDecoder->process();

		ip_pStimulationsToEncode1->clear();

		uint64 l_ui64ChunkStartTime=l_rDynamicBoxContext->getInputChunkStartTime(0, i);
		uint64 l_ui64ChunkEndTime  =l_rDynamicBoxContext->getInputChunkEndTime(0, i);
		uint64 l_ui64sampleTime = l_ui64ChunkEndTime;

		//HEADER
		if(m_pSignalDecoder->isOutputTriggerActive(OVP_GD_Algorithm_SignalStreamDecoder_OutputTriggerId_ReceivedHeader))
		{
		}

		//BUFFER
		if(m_pSignalDecoder->isOutputTriggerActive(OVP_GD_Algorithm_SignalStreamDecoder_OutputTriggerId_ReceivedBuffer))
		{
			TParameterHandler<IMatrix*> ip_pMatrix(m_pSignalDecoder->getOutputParameter(OVP_GD_Algorithm_SignalStreamDecoder_OutputParameterId_Matrix));
			itpp::mat X = convert(*ip_pMatrix);

			itpp::mat P; //covariance matrix of the input

			if (m_IsP300)
			{
				//concatenate 
				//[P1]
				//[X1]
				itpp::mat XC = itpp::concat_vertical(m_mP1,X);

				P = itpp::cov(XC.transpose(),false);
			}
			else
			{
				P = itpp::cov(X.transpose(),false);
			}

			if (m_vOnlineClasses.size()>0 && m_vOnlineClasses[0]->ResultMean.rows() != P.rows())
			{
				this->getLogManager() << LogLevel_Error << "Problem with matrices size! Probably the number of channels for the online is not the same as it was for the training." << "\n";
				return false;
			}

			std::vector<OpenViBE::float64> l_vDistances;

			//construct parameter with the means from all classes
			vector_type<itpp::mat> l_vResultMean;
			for(int i=0;i<m_vOnlineClasses.size();i++)
			{
				l_vResultMean.push_back(m_vOnlineClasses[i]->ResultMean);
			}

			//get resulting class
			OpenViBE::uint32 index = ApplyMDM(P, l_vResultMean, l_vDistances);

			//start output stimulations
			OpenViBE::uint64 l_StimId;
			l_StimId = m_vClassStimulations[index];//we select the stimulation for the best class

			ip_pStimulationsToEncode1->appendStimulation(l_StimId,l_ui64sampleTime, 0);

			if (ip_pStimulationsToEncode1->getStimulationCount()>0)
			{
				op_pEncodedMemoryBuffer1=l_rDynamicBoxContext->getOutputChunk(0);
				m_pStimulationEncoder1->process(OVP_GD_Algorithm_StimulationStreamEncoder_InputTriggerId_EncodeBuffer);
				l_rDynamicBoxContext->markOutputAsReadyToSend(0,l_ui64ChunkStartTime ,l_ui64ChunkEndTime );
			}
			//end output stimulations

			//start output filtered stimulation
			index = LabelFilter(l_vDistances);
			if (index!=-1)
			{
				OpenViBE::uint64 l_StimId;
				l_StimId = m_vClassStimulations[index];//we select the stimulation for the best class

				ip_pStimulationsToEncode2->appendStimulation(l_StimId,l_ui64sampleTime, 0);

				if (ip_pStimulationsToEncode2->getStimulationCount()>0)
				{
					op_pEncodedMemoryBuffer2=l_rDynamicBoxContext->getOutputChunk(2);
			        m_pStimulationEncoder2->process(OVP_GD_Algorithm_StimulationStreamEncoder_InputTriggerId_EncodeBuffer);
			        l_rDynamicBoxContext->markOutputAsReadyToSend(2,l_ui64ChunkStartTime ,l_ui64ChunkEndTime );
				}
			}
			else
			{
				this->getLogManager() << LogLevel_Warning << "Skipped classification.\n";
			}
			//end output filtered stimulation

		    //start output score 
			ip_pMatrixToEncode->setDimensionCount(1);
			ip_pMatrixToEncode->setDimensionSize(0,l_vDistances.size());
			
			System::Memory::copy(
			ip_pMatrixToEncode->getBuffer(),
			&l_vDistances[0],
			l_vDistances.size()*sizeof(float64));

			if(!m_bHasSentHeader)
			{
				op_pEncodedMemoryBuffer=l_rDynamicBoxContext->getOutputChunk(1);
				m_pSignalEncoder->process(OVP_GD_Algorithm_StreamedMatrixStreamEncoder_InputTriggerId_EncodeHeader);
				l_rDynamicBoxContext->markOutputAsReadyToSend(1,l_ui64ChunkStartTime, l_ui64ChunkEndTime);
				m_bHasSentHeader=true;
			}

			op_pEncodedMemoryBuffer=l_rDynamicBoxContext->getOutputChunk(1);			
			m_pSignalEncoder->process(OVP_GD_Algorithm_StreamedMatrixStreamEncoder_InputTriggerId_EncodeBuffer);
			l_rDynamicBoxContext->markOutputAsReadyToSend(1, l_ui64ChunkStartTime, l_ui64ChunkEndTime);
			//end output score
		}

		l_rDynamicBoxContext->markInputAsDeprecated(0, i);
	}

	return true;
}

itpp::mat CBoxAlgorithmProcessMDM::convert(const OpenViBE::IMatrix& rMatrix)
{
	itpp::mat l_oResult(
		rMatrix.getDimensionSize(1),
		rMatrix.getDimensionSize(0));

	System::Memory::copy(l_oResult._data(), rMatrix.getBuffer(), rMatrix.getBufferElementCount()*sizeof(float64));
	return l_oResult.transpose();
}

OpenViBE::boolean CBoxAlgorithmProcessMDM::loadFile(std::string l_sFileNamePath)
{
	//processing parameter file
	if (l_sFileNamePath == "") 
	{ 
	    this->getLogManager() << LogLevel_Error << "Training parameters file field is empty!\n";
		return false;
	}
	else
	{
		this->getLogManager() << LogLevel_Info << "Parameters file:" << l_sFileNamePath.c_str() << "\n";

		m_ParamFile.open(l_sFileNamePath.c_str());

		if (m_ParamFile.bad())
		{
			this->getLogManager() << LogLevel_Error << "Could not read training parameters file:" << l_sFileNamePath.c_str() << "\n";
			return false;
		}
		else
		{
			OpenViBE::uint32 count=0; //all including P1

			char name[50];
			m_ParamFile >> name >> count;

			for (int i=0;i<count;i++)
			{
				itpp::mat X;
				m_ParamFile >> name >> X;

				if (X.cols() == 0  || X.rows() == 0)
				{
					this->getLogManager() << LogLevel_Error << "Problem with matrix!\n";
					return false;
				}

				if (std::string(name) == std::string("P1"))
				{
					m_mP1 = X;
					this->getLogManager() << LogLevel_Info << "P1 detected!\n";
				}
				else
				{
					RClass* c = new RClass();
					c->ResultMean = X;
					m_vOnlineClasses.push_back(c);
					//m_vMean.push_back(X);
				}
			}

			//read mean std
			//m_ParamFile >> name;

			std::cout << std::endl;

			std::string line;
			std::getline(m_ParamFile, line);

			int i =0 ;
			while(std::getline(m_ParamFile, line)) 
			{
				std::string delimiters("=;");
                std::vector<std::string> parts;
                boost::split(parts, line, boost::is_any_of(delimiters));

				m_vOnlineClasses[i]->DistanceMean = atof(parts[1].c_str());
				m_vOnlineClasses[i]->DistanceSTD = atof(parts[2].c_str());
				std::cout << m_vOnlineClasses[i]->DistanceMean << " " << m_vOnlineClasses[i]->DistanceSTD << std::endl;
				i++;
			}

			if (m_vOnlineClasses.size()==0)
			{
				this->getLogManager() << LogLevel_Error << "No class matrices loaded from file!\n";
				return false;
			}
			else
			if (m_IsP300 && (m_vOnlineClasses.size()+1) != count) 
				this->getLogManager() << LogLevel_Warning << "The number of matrices loaded is different from what is specified in the file!\n";

			else if (!m_IsP300 && m_vOnlineClasses.size() != count) 
				this->getLogManager() << LogLevel_Warning << "The number of matrices loaded is different from what is specified in the file!\n";

			else if (m_IsP300 && (m_mP1.rows() == 0 || m_mP1.cols() == 0))
				this->getLogManager() << LogLevel_Warning << "P1 matrix for P300 expected, but not found!\n";

			else if (!m_IsP300 && (m_mP1.rows() != 0 || m_mP1.cols() != 0))
				this->getLogManager() << LogLevel_Warning << "P1 matrix not expected, but detected!\n";

			else
			this->getLogManager() << LogLevel_Info << "Training parameters file loaded successfully!\n";

			m_ParamFile.close();
		}
	}

	return true;
    //end of processing file
}

OpenViBE::uint32 CBoxAlgorithmProcessMDM::ApplyMDM(itpp::mat P,vector_type<itpp::mat> vMean,std::vector<OpenViBE::float64>& vDistances)
{
	OpenViBE::float64 minValue = Riemann::distance(P,vMean[0]);
	OpenViBE::uint32 minPos = 0; //class index
	vDistances.push_back(minValue);

	for(int j=1;j<vMean.size();j++) //we find the min distance and we also store all the distances for later output
	{
		//std::cout << j << std::endl;
		//std::cout << m_vMean[j].cols() << " " << m_vMean[j].cols() << std::endl;
		OpenViBE::float64 d = Riemann::distance(P,vMean[j]);

		vDistances.push_back(d);
		if (d < minValue) 
		{
			minValue = d;
			minPos = j;
		}
	}

	return minPos;
}

OpenViBE::uint32 CBoxAlgorithmProcessMDM::LabelFilter(std::vector<OpenViBE::float64> d)
{
	//d the distances to all classes for the specific incoming sample
	int max = 1000000;

	itpp::vec dd(d.size());
	for(int i=0;i<d.size();i++)
	{
	  dd[i] = (double)d[i];
	  dd[i] = log(dd[i]);
	}

	for(int i=0;i<m_vOnlineClasses.size();i++)
	{
		double p = normcdf(dd(i),m_vOnlineClasses[i]->DistanceMean,m_vOnlineClasses[i]->DistanceSTD);

		if ( (1 - p) < 0.05) 
                dd[i] = max; //make it big so that we can exclude it
            
	}

	//find min
	OpenViBE::float64 minValue = dd[0];
	OpenViBE::uint32 minPos = 0; //class index

	for(int j=1;j<m_vOnlineClasses.size();j++) //we find the min distance and we also store all the distances for later output
	{
		double m = dd[j];

		if (m < minValue) 
		{
			minValue = m;
			minPos = j;
		}
	}

	if (dd[minPos] == max)
	return -1; //no class was good enough
	else return minPos;
}

double CBoxAlgorithmProcessMDM::normcdf(double x,double mu,double sigma)
{
	// constants
    double a1 =  0.254829592;
    double a2 = -0.284496736;
    double a3 =  1.421413741;
    double a4 = -1.453152027;
    double a5 =  1.061405429;
    double p  =  0.3275911;

    // Save the sign of x
    int sign = 1;
    if (x < mu)
       sign = -1;
    x = fabs(x-mu)/sqrt(2.0*sigma*sigma);

    // A&S formula 7.1.26
    double t = 1.0/(1.0 + p*x);
    double y = 1.0 - (((((a5*t + a4)*t) + a3)*t + a2)*t + a1)*t*exp(-x*x);

    return 0.5*(1.0 + sign*y);
}
